package com.zzs.path.util;

import com.zzs.path.entity.Node;
import com.zzs.path.entity.AStarNode;
import com.zzs.path.entity.Route;

import java.util.ArrayList;
import java.util.HashSet;
import java.util.List;
import java.util.Set;

public class AStarUtils {

    /**
     * 利用 AStar 算法，计算两个节点间的路径（尽量最短）
     * @param graph 地图
     * @param start 开始的index
     * @param end 结束的index
     * @param nodes 节点集合，包含位置信息
     * @return index路径、路径长度
     */
    public static Route aStar(int graph[][], int start, int end, List<Node> nodes){
        System.out.println();
        System.out.println("AStartUtils aStar ：计算"+start+"到"+end+"的路径" );
        int num = nodes.size();
        Set<AStarNode> close = new HashSet<>();
        List<AStarNode> open = new ArrayList<>();
        AStarNode lastNode = null;

        AStarNode startNode = new AStarNode(start, nodes.get(start) , 0, 0, null);
        open.add(startNode);
        boolean finish = false;

        while( !finish && !open.isEmpty() ){
            // 1、找到 f 最小的节点ln ，从 open 移动到 close
            lastNode = getMinFromOpen(open);
            close.add(lastNode);

            // 2、找 ln 所有的 邻居节点nn
            int index = lastNode.getIndex();
            for(int i = 0; i<num; i++){
                if(graph[index][i] > 0){
                    if( i == end){ // 如果是终点
                        finish = true;
                        break;
                    }
                    AStarNode neighborNode = new AStarNode(
                            i, nodes.get(i),
                            lastNode.getG() + graph[index][i],
                            h(nodes.get(i), nodes.get(end)),
                            lastNode);
                    if(close.contains(neighborNode)){
                        continue;
                    }
                    AStarNode otherNode = getNodeFromOpen(open, neighborNode);
                    if(otherNode != null){
                        if(lastNode.getG() + graph[index][i] < otherNode.getG()){
                            otherNode.setG( lastNode.getG() + graph[index][i] );
                            otherNode.setParent(lastNode);
                        }
                    }else{
                        open.add(neighborNode);
                    }
                }
            }
        }

        Route route = null;
        if(finish){
            int distance = graph[lastNode.getIndex()][end] + lastNode.getG();
            List<Integer> routeList = new ArrayList<>();
            routeList.add(end);
            while(lastNode.getIndex() != start){
                routeList.add(lastNode.getIndex());
                lastNode = lastNode.getParent();
            }
            routeList.add(lastNode.getIndex()); // 此时，存放的是相反的路径

            route = new Route(distance, routeList);
            System.out.println("成功找到路径："+route);
        }else{
            System.out.println("没有找到路径");
        }
        return route;
    }

    /**
     * 从 open 列表中获取 节点
     * @param open
     * @param node
     * @return 获取该节点用于判断 g 是否合适，是否需要更新
     */
    public static AStarNode getNodeFromOpen(List<AStarNode> open, AStarNode node){
        for(int i = 0;i<open.size();i++){
            if(open.get(i).equals(node)){
                return open.get(i);
            }
        }
        return null;
    }

    /**
     * 从 open 中找到 f 最小的，移除并返回
     */
    public static AStarNode getMinFromOpen(List<AStarNode> open){
        int f = Integer.MAX_VALUE;
        int index = -1;
        for(int i = 0;i<open.size();i++){
            AStarNode temp = open.get(i);
            if(temp.f() < f){
                f = temp.f();
                index = i;
            }
        }
        return open.remove(index);
    }


    // 计算h，即评估函数
    private static int h(Node n1, Node n2){
        return hManhattanDistance(n1, n2);
        //return hPowEuclidianDistance(n1, n2);
        //return hBFS(n1, n2);
    }

    /**
     * 曼哈顿距离,小于等于实际值
     */
    private static int hManhattanDistance(Node n1, Node n2)
    {
        return Math.abs(n1.getX() - n2.getX()) + Math.abs(n1.getY() - n2.getY());
    }

    /**
     * 欧式距离平方,大于等于实际值
     */
    private static int hPowEuclidianDistance(Node n1, Node n2)
    {
        return (int)(Math.pow(n1.getX() - n2.getX(), 2) + Math.pow(n1.getY() - n2.getY(), 2));
    }

    /**
     * BFS的h值,恒为0
     */
    private static int hBFS(Node n1, Node n2)
    {
        return 0;
    }

}
